#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/server/simple_action_server.h>
#include <arm_navigation_msgs/MoveArmAction.h>
#include <arm_navigation_msgs/utils.h>

#include <knobs_rev/PoseArmAction.h>

using namespace ros;
using namespace actionlib;


class PoseArmServer {
public:
  PoseArmServer();
  ~PoseArmServer();

  void callback(const knobs_rev::PoseArmGoalConstPtr &goal);

private:
  NodeHandle m_node;
  SimpleActionServer<knobs_rev::PoseArmAction> m_action_server;
  SimpleActionClient<arm_navigation_msgs::MoveArmAction> m_move_arm;
};


PoseArmServer::PoseArmServer() :m_action_server(m_node, "pose_arm", boost::bind(&PoseArmServer::callback, this, _1), false), m_move_arm("move_right_arm", true) {
  m_move_arm.waitForServer();

  // Start the action server
  m_action_server.start();

  ROS_INFO("Pose arm server ready");
}


PoseArmServer::~PoseArmServer() {
}


void PoseArmServer::callback(const knobs_rev::PoseArmGoalConstPtr &goal) {
  arm_navigation_msgs::SimplePoseConstraint pose;
  arm_navigation_msgs::MoveArmGoal actual_goal;

  // Invariants in the goal message
  actual_goal.motion_plan_request.group_name = "right_arm";
  actual_goal.motion_plan_request.num_planning_attempts = 1;
  actual_goal.motion_plan_request.planner_id = std::string("");
  actual_goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  actual_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  // Invariants in the pose message
  pose.link_name = "r_wrist_roll_link";
  pose.absolute_position_tolerance.x = 0.02;
  pose.absolute_position_tolerance.y = 0.02;
  pose.absolute_position_tolerance.z = 0.02;
  pose.absolute_roll_tolerance = 0.04;
  pose.absolute_pitch_tolerance = 0.04;
  pose.absolute_yaw_tolerance = 0.04;
  
#if 0
  pose.header.frame_id = "torso_lift_link";
  pose.pose.position.x = 0.78;
  pose.pose.position.y = -0.188;
  pose.pose.position.z = 0.09;
#endif

#if 1
  pose.header.frame_id = goal->pose.header.frame_id;
  pose.pose = goal->pose.pose;
#endif

//  pose.pose.orientation.x = 0.0;
  //pose.pose.orientation.y = 0.0;
  //pose.pose.orientation.z = 0.0;
  //pose.pose.orientation.w = 1.0;

  ROS_INFO("Moving to (%f, %f, %f)", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z);

  // Populate the goal message
  arm_navigation_msgs::addGoalConstraintToMoveArmGoal(pose, actual_goal);

  // Send the message
  m_move_arm.sendGoal(actual_goal);
  m_move_arm.waitForResult();

  // Pass the success code through
  m_action_server.setSucceeded();
}


int main(int argc, char **argv){
  init (argc, argv, "pose_arm");

  PoseArmServer server;

  spin();
  
  return 0;
}




#if 0
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
#endif
